#run get_contour_v2_ok.py first, then this script to generate ESP32 Nav Json地图
import cv2
import numpy as np
import json
import matplotlib.pyplot as plt

# --- 1. 从掩模图片生成json地图文件 ---
MASK_IMAGE_FILE = "d:/F/Open Source/Prj_Robdog/xiaozhi-esp32-server/my_mcp/yolo8/ESP32_Slam/mask.png"    # 你的掩模图片
JSON_MAP_FILE = "d:/F/Open Source/Prj_Robdog/xiaozhi-esp32-server/my_mcp/yolo8/ESP32_Slam/esp32_nav_map.json"

# 读取mask图像（白色可行走，黑色障碍！！）
mask = cv2.imread(MASK_IMAGE_FILE, cv2.IMREAD_GRAYSCALE)
if mask is None:
    raise ValueError(f"无法读取文件：{MASK_IMAGE_FILE}")

# 白色为可走，黑色为障碍（你的实际情况！）
grid_data = np.where(mask > 127, 0, 100).astype(np.uint8)
grid_data_flat = grid_data.flatten().tolist()

# 组合json格式
nav_map = {
    "width": mask.shape[1],
    "height": mask.shape[0],
    "resolution": 0.2,    # 每像素1厘米，按你的实际地图比例设置
    "origin": [0, 0],
    "data": grid_data_flat
}

with open(JSON_MAP_FILE, 'w') as f:
    json.dump(nav_map, f)
print(f"导航JSON地图已保存到: {JSON_MAP_FILE}")

# --- 2. json中读取并matplotlib显示 ---
with open(JSON_MAP_FILE, 'r') as f:
    json_map = json.load(f)

arr = np.array(json_map['data'], dtype=np.uint8).reshape((json_map['height'], json_map['width']))

plt.figure(figsize=(8,10))
plt.imshow(arr, cmap='gray', origin='upper')
plt.title('ESP32 Navigation Map\n(0 = free, 100 = obstacle)')
plt.axis('off')
plt.tight_layout()
plt.show()